/**
 * \file
 * \date 18.09.2013
 * \author alex
 */
#ifndef bullet_rviz_test_PHYSICS_H_
#define bullet_rviz_test_PHYSICS_H_

#include <vector>
#include <ros/time.h>
#include <bullet_rviz_test/physical_object.h>

// Bullet class forward declarations:
class btBroadphaseInterface;
class btCollisionConfiguration;
class btCollisionDispatcher;
class btSequentialImpulseConstraintSolver;
class btDynamicsWorld;

namespace bullet_rviz_test
{

/**
 * Abstraction of physics engine initialization / simulation / cleanup.
 */
class Physics
{
public:
  Physics();
  ~Physics();

  btDynamicsWorld * getWorld();
  void stepSimulation();
  void stepSimulation(double time_step);

  void addPhysicalObject(const PhysicalObjectPtr & physical_object);
  void removePhysicalObject(const PhysicalObjectPtr & physical_object);

private:
  typedef std::vector<PhysicalObjectPtr> PhysicalObjectVector;

  Physics(const Physics &);
  Physics & operator=(const Physics &);

  btBroadphaseInterface * broadphase_;
  btCollisionConfiguration * collision_configuration_;
  btCollisionDispatcher * collision_dispatcher_;
  btSequentialImpulseConstraintSolver * constraint_solver_;
  btDynamicsWorld * world_;

  PhysicalObjectVector physical_objects_;
  ros::Time last_time_;
};

typedef boost::shared_ptr<Physics> PhysicsPtr;

}

#endif /* bullet_rviz_test_PHYSICS_H_ */
